%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%% PID controller %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K=20/1700/2*6;
Ti=0;
Td=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%% PUMA parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%
latency=0.01;
Tpuma=0.01;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% pneumatic motor params %%%%%%%%%%%%%%%%%%%%%%%
r0=7;   %rotor radius, mm
l=28;   %rotor lenght, mm
e=1.2;  %excentricity, mm
phi0=30; %input end angle, grad
Pat=9;   %preassure, at (technical)
n0=10000;   % spindle speed, rpm
Mfric=0;  %friction torque, N*m
Jcutter=0; %cutter inertia, Kg*m^2

a=r0/e;
C1=a-(a+1)*cosd(phi0)+cosd(phi0)^2;
Se=10^(-9)*l*r0^2/(a^2)*C1; %Semin

P=Pat*98066.5;
M=P*Se;
rho=7.8*10^3; % kg/m^3
J=3*0.95*10^(-15)*pi/2*r0^4*l*rho+Jcutter;
omega=1.6*n0/60*2*pi;
Bk=M/(omega^2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%  trajectory sample %%%%%%%%%%%%%%%%%%%%%%%%%%
xx=[  0 10 20 30 40 45 50 60 70 80 90 100 110 120];
yy=[ 55 55 10 10  5  5 52 52 20 20 35  35   5   5];
bb=55-yy;
cosa=delta(xx,yy);
